import os,sys,time
import RPi.GPIO as GPIO



class Car(object):
	def __init__(self):
		self.pinid_in1p = 3
		self.pinid_in1n = 5
		self.pinid_in2p = 7
		self.pinid_in2n = 8

		self.pinid_pwm1 = 10
		self.pinid_pwm2 = 11

		frequence = 100

		GPIO.setmode(GPIO.BOARD)

		GPIO.setup(self.pinid_in1p,GPIO.OUT)
		GPIO.setup(self.pinid_in1n,GPIO.OUT)
		GPIO.setup(self.pinid_in2p,GPIO.OUT)
		GPIO.setup(self.pinid_in2n,GPIO.OUT)

		GPIO.setup(self.pinid_pwm1,GPIO.OUT)
		GPIO.setup(self.pinid_pwm2,GPIO.OUT)

		self.pin_pwm1 = GPIO.PWM(self.pinid_pwm1, frequence)
		self.pin_pwm2 = GPIO.PWM(self.pinid_pwm2, frequence)

		self.pin_pwm1.start(0)
		self.pin_pwm2.start(0)

	def go_forward(self):
		self._2m_forward()
		self.pin_pwm1.ChangeDutyCycle(100)
		self.pin_pwm2.ChangeDutyCycle(100)

	def go_backward(self):
		self._2m_backward()
		self.pin_pwm1.ChangeDutyCycle(100)
		self.pin_pwm2.ChangeDutyCycle(100)


	def go(self,pitch,roll):
		if pitch > 500 or pitch < -500:
			pitch = 500 if pitch > 0 else -500

		if roll > 500 or roll < -500:
			roll = 500 if roll > 0 else -500

		if pitch > 0:
			self._2m_forward()
		else:
			self._2m_backward()

		power = (abs(pitch) / 500.0) * 100
		offset = (roll / 500.0) * 35

		print 'power: '+str(power) + ', offset: '+str(offset)

		power_m1 = power + offset
		power_m2 = power - offset

		if power_m1 > 100 or power_m1 < 0:
			power_m1 = 100 if power_m1 > 100 else 0
		if power_m2 > 100 or power_m2 < 0:
			power_m2 = 100 if power_m2 > 100 else 0

		print 'm1: '+str(power_m1) + '; m2: '+str(power_m2)

		self.pin_pwm1.ChangeDutyCycle(power_m1)
		self.pin_pwm2.ChangeDutyCycle(power_m2)

	def stop(self):
		GPIO.output(self.pinid_in1p,False)
		GPIO.output(self.pinid_in1n,False)
		GPIO.output(self.pinid_in2p,False)
		GPIO.output(self.pinid_in2n,False)

		self.pin_pwm1.ChangeDutyCycle(0)
		self.pin_pwm2.ChangeDutyCycle(0)

	def poweroff(self):
		self.stop()
		self.pin_pwm1.stop()
		self.pin_pwm2.stop()
		GPIO.cleanup()

	def _2m_forward(self):
		GPIO.output(self.pinid_in1p,True)
		GPIO.output(self.pinid_in1n,False)
		GPIO.output(self.pinid_in2p,True)
		GPIO.output(self.pinid_in2n,False)

	def _2m_backward(self):
		GPIO.output(self.pinid_in1p,False)
		GPIO.output(self.pinid_in1n,True)
		GPIO.output(self.pinid_in2p,False)
		GPIO.output(self.pinid_in2n,True)


if __name__ == '__main__':
	car = Car()
	car.forward()
